

In [1]:

    
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
from pylab import imread
%matplotlib inline
from mpl_toolkits.mplot3d import Axes3D

Expérience triangulation dans une image

Préparatifs

On place un long multiprise dans le champ de la caméra, de sorte que les extrémités du multiprise coïncident avec les bords gauche et droit de l'image captée par la caméra.

On place une tige verticale sur le côté de la caméra, et une lumière vive derrière la tige, afin de projeter l'ombre de la tige sur le milieu du multiprises, matérialisé sur les photos par une bouteille de Club Mate.

On mesure différentes distances parmis les objets ainsi placés:

Entre l'objectif de la caméra et la face visible (avant) du multiprises: 58cm
La longueur du multiprises: 69cm
La distance latérale de la tige à la caméra:
La distance entre la tige et le multiprises:

Expérience

On prend plusieurs photos de la scène ainsi formée en y plaçant un objet (une boîte en carton) à différents endroits. Lors de chaque photo, on note la distance entre l'avant de l'objet et l'avant du multiprises. Cette distance, en cm, donne le nom de l'image (exemple: 21cm devant le multiprises = 21.jpg, 11.5cm = 11-5.jpg).

Sur les images ainsi obtenues, nous marquons d'une ligne rouge l'ombre sur la boîte. Un programme doit trianguler la position de l'objet, et la position ainsi obtenue doit correspondre (dans les limites de précision peu élevées correspondant aux mesures prises) à la position mesurée (le nom de l'image).

Images



In [11]:

    
name = "21"
img = imread(name + ".bmp")
img_red = imread(name + "+red.bmp")



In [12]:

    
diff = img_red - img
plt.imshow(img_red)
plt.show()
plt.imshow(diff)









    












    Out[12]:





<matplotlib.image.AxesImage at 0x7fe79e6485d0>

Calcul vectoriel d'une position sur l'image (2D)

$\theta$, l'angle que fait le rayon sortant de la caméra avec la bisectrice de l'ouverture de la caméra
$ A = \begin{pmatrix} i \\ j \end{pmatrix} $, la position du laser
$ B = \begin{pmatrix} 0 \\ k \end{pmatrix}$, point où le laser est au centre de l'image
$ P' = \begin{pmatrix} k \tan(\theta) \\ k \end{pmatrix} $, le point visible sur le plan de l'image (projection du point visible sur le plan orthogonal à $B$)

Nous savons que le point est à l'intersection du laser et du rayon sortant de la caméra, donc

$ A + \lambda (B - A) - \mu P' = 0 $

$ \begin{pmatrix} i \\ j \end{pmatrix} + \lambda \begin{pmatrix} -i \\ k-j \end{pmatrix} - \mu \begin{pmatrix} k\tan(\theta) \\ k \end{pmatrix} \Leftrightarrow \begin{pmatrix} i & k \tan(\theta) \\ j-k & k \end{pmatrix} \begin{pmatrix} \lambda \\ \mu \end{pmatrix} = \begin{pmatrix} i \\ j \end{pmatrix} $



In [13]:

    
from math import tan
k = 58.0            # Distance de la camera au point ou le laser est au centre de l'image (cm)
i, j = -45.0, 26.5  #Position du laser par rapport à la camera

A, B = np.array([i,j]), np.array([0,k])
Ax, Ay = A
Bx, By = B

# (x,y)
def pos(theta):
    matrix = np.array([[Ax, By*tan(theta)], [Ay-By, By]])
    l, m = np.linalg.solve(matrix, A)
    return A + l*(B-A)

Calcul de l'angle (θ)

Notons φ l'angle maximal entre le centre et le bord (droit) de l'image
Notons E la largeur de l'image en pixels, e la distance en pixels entre le centre de l'image et l'image de p
Notons m la largeur réelle entre les 2 bords observables en b

Principe

m = 2*k*tan(φ)  => φ = atan(m/2k)
θ = φ * e/(E/2)



In [14]:

    
from math import atan, pi
m = 69
phi = atan(float(m)/(2*k))
E = diff.shape[1]
print "Ouverture camera:", 180*phi/pi, "degrés. Points détectés:"

theta = lambda e: phi * e/(E/2)

X, Y = [], []
XY = []
i = 260
j_save = 0
for j in range(diff.shape[1]):
    if tuple(diff[i][j]) != (0, 0, 0):
        j_save = j
        t = theta(j - E/2)
        XY.append(pos(t))
        print XY[-1]

X, Y = map(np.array, zip(*XY))









    



Ouverture camera: 30.7453492818 degrés. Points détectés:
[-15.74785603  46.97650078]
[-15.68474116  47.02068119]
[-15.62157038  47.06490074]
[-15.55834332  47.10915968]

Visualisation du résultat

Affichage en 3D (hauteur à 0 pour le moment)



In [17]:

    
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d', aspect="equal")
ax.scatter(X, Y, [0 for i in range(len(X))])









    Out[17]:





<mpl_toolkits.mplot3d.art3d.Path3DCollection at 0x7fe79e3fc750>

Affichage en 2D

Rayon laser (équation vectorielle établie à partir des mesures), en rouge
Rayon capté par la caméra, en jaune
Champ de vue de la caméra (vert)
Point détecté (points bleus)
Multiprise (en noir)



In [18]:

    
#shortcut for vector
vec = lambda *args : np.array(args)

fig = plt.subplot(111, aspect='equal')
# Computed points from above
fig.scatter(X, Y)

def line(direction, position=np.array([0, 0]), **kwargs):
    """Draw a parametric line"""
    xy = [position + t*direction for t in np.linspace(0, 1)]
    x, y = zip(*xy)
    fig.plot(x, y, **kwargs)

# Image background
line(vec(m, 0), position=vec(-m/2, k), color='black', linewidth=3)
    
# A ray from the camera
line(pos(theta(j_save - E/2)), color='y')

# Camera left limit
line(vec(tan(theta(-E/2))*k, k), color='g')

# Camera right limit
line(vec(tan(theta(E/2))*k, k), color='g')

#Laser ray
line(B-A, A, color='r')

#System parameters
line(B, color='m', ls='--')
line(A*vec(1, 0), color='m', ls='--')
line(A*vec(0, 1), A*vec(1, 0), color='m', ls='--')

fig.set_aspect('equal')
plt.show()
plt.imshow(img_red)









    












    Out[18]:





<matplotlib.image.AxesImage at 0x7fe79e584910>

Vue du laser par la caméra

Si on balaye de gauche à droite avec la caméra (variation de θ), voici ce que l'on peut détecter (vu de haut)



In [210]:

    
from math import pi
XY = [pos(t) for t in np.linspace(-phi, phi)]
X, Y = map(np.array, zip(*XY))
plt.xlim(-m/2, m/2)
plt.ylim(0, 2*k)
plt.scatter(X, Y)









    Out[210]:





<matplotlib.collections.PathCollection at 0x7fa65d3972d0>



In [110]:



In [ ]: